import imp
import threading
from os import stat
from socket import socket
import pickle

from numpy import pad
import remote_con_utilty as rcu
import time

dead_line = 10
safe_distance = 20

from status import Status
from inter_status import InterStatus


class Car:
    def __init__(self, addr):
        self.remote = rcu.SyncThreading(addr)
        self.remote.set_completed_callback(self.handler_completed)
        self.remote.start()
        self.manual_idle = False
        self.most_left_edge = 1000
        self.most_right_edge = 0

        self.inter_status = InterStatus()

    def restore(self):
        """恢复"""
        pass

    def handler_completed(self, flag):
        print(f'on handler_completed: flag = {flag}')
        self.inter_status.turn += 1
        self.inter_status.curr_status = self.padding_from

    def update_bound_edge(self, coor):
        self.most_left_edge = 1000
        self.most_right_edge = 0
        for o in coor:
            if o[1] < self.most_left_edge:
                self.most_left_edge = o[1]
            if o[3] > self.most_right_edge:
                self.most_right_edge = o[3]
        # print(f'most left {self.most_left_edge} most right  {self.most_right_edge}')

    def get_next_status(self, coor, distance):
        if distance < 0:
            print('zero report')
            return
        self.update_bound_edge(coor)
        if self.inter_status.curr_status == Status.idle:
            if distance >= safe_distance:
                self.inter_status.curr_status = Status.heading
            elif dead_line < distance < safe_distance:
                self.inter_status.curr_status = Status.avoding
            elif distance <= dead_line:
                self.inter_status.curr_status = Status.fallback

        elif self.inter_status.curr_status == Status.heading:
            if distance >= safe_distance:
                if self.inter_status.forward_count > 0 or self.inter_status.turn > 0:
                    self.inter_status.curr_status = Status.avoding  # 如该已经在避障了则继续
                else:
                    self.inter_status.curr_status = Status.heading
            elif dead_line < distance < safe_distance:
                if len(coor) > 0:
                    self.inter_status.curr_status = Status.avoding
                else:
                    self.remote.clear()
                    self.inter_status.curr_status = Status.padding
            elif distance <= dead_line:
                self.inter_status.curr_status = Status.fallback

        elif self.inter_status.curr_status == Status.fallback:
            if distance >= safe_distance:
                if self.inter_status.forward_count > 0 or self.inter_status.turn > 0:
                    self.inter_status.curr_status = Status.avoding  # 如该已经在避障了则继续
                else:
                    self.inter_status.curr_status = Status.heading
            elif dead_line < distance < safe_distance:
                self.inter_status.curr_status = Status.avoding
            elif distance <= dead_line:
                self.inter_status.curr_status = Status.fallback

        elif self.inter_status.curr_status == Status.avoding:
            if distance <= dead_line:
                self.inter_status.curr_status = Status.fallback
            else:
                if self.most_left_edge >= (640 - self.most_right_edge):
                    self.padding_from = Status.left_move_stat
                    self.inter_status.curr_status = Status.left_move_stat
                else:
                    self.padding_from = Status.right_move_stat
                    self.inter_status.curr_status = Status.right_move_stat

    def right_move(self):
        if self.inter_status.turn < 3:
            self.remote.inplace_right()
            self.inter_status.curr_status = Status.padding
        elif self.inter_status.forward_count < 40:
            self.remote.forward()
            print('forwarding phase 1')
            self.inter_status.forward_count += 1
        elif self.inter_status.turn < 6:
            self.remote.inplace_left()
        elif self.inter_status.forward_count < 50:
            self.remote.forward()
            print('forwarding phase 2')
            self.inter_status.forward_count += 1
        elif self.inter_status.turn < 8:
            self.remote.inplace_left()
            self.inter_status.curr_status = Status.padding
        elif self.inter_status.forward_count < 66:
            self.remote.forward()
            print('forwarding phase 3')
            self.inter_status.forward_count += 1
        elif self.inter_status.turn < 10:
            self.remote.inplace_right()
            self.inter_status.curr_status = Status.padding
        elif self.inter_status.forward_count < 70:
            self.remote.forward()
            print('forwarding phase 4')
            self.inter_status.forward_count += 1
        else:
            self.remote.clear()
            self.inter_status.forward_count = 0
            self.inter_status.turn = 0
            self.inter_status.curr_status = Status.heading

    def left_move(self):
        if self.inter_status.turn < 3:
            self.remote.inplace_left()
            self.inter_status.curr_status = Status.padding
        elif self.inter_status.forward_count < 40:
            self.remote.forward()
            print('forwarding phase 1')
            self.inter_status.forward_count += 1
        elif self.inter_status.turn < 6:
            self.remote.inplace_right()
        elif self.inter_status.forward_count < 50:
            self.remote.forward()
            print('forwarding phase 2')
            self.inter_status.forward_count += 1
        elif self.inter_status.turn < 8:
            self.remote.inplace_right()
            self.inter_status.curr_status = Status.padding
        elif self.inter_status.forward_count < 66:
            self.remote.forward()
            print('forwarding phase 3')
            self.inter_status.forward_count += 1
        elif self.inter_status.turn < 10:
            self.remote.inplace_left()
            self.inter_status.curr_status = Status.padding
        elif self.inter_status.forward_count < 70:
            self.remote.forward()
            print('forwarding phase 4')
            self.inter_status.forward_count += 1
        else:
            self.remote.clear()
            self.inter_status.forward_count = 0
            self.inter_status.turn = 0
            self.inter_status.curr_status = Status.heading

    def do_status(self):
        print(Status.status_map[self.inter_status.curr_status])
        if self.inter_status.curr_status == Status.idle:
            self.remote.clear()
        elif self.inter_status.curr_status == Status.heading:
            self.remote.forward()
        elif self.inter_status.curr_status == Status.fallback:
            self.remote.backward()
        elif self.inter_status.curr_status == Status.avoding:
            # self.remote.clear()
            pass
        elif self.inter_status.curr_status == Status.left_move_stat:
            self.left_move()
        elif self.inter_status.curr_status == Status.right_move_stat:
            self.right_move()

    # 一个根据坐标物体的避障函数
    def auto_mode(self, coor, distance):
        if self.manual_idle:
            self.remote.clear()
            return
        self.get_next_status(coor, distance)
        self.do_status()

    def manual_forward(self):
        self.remote.forward()

    def manual_backward(self):
        self.remote.backward()
    
    def manual_stop(self):
        self.manual_idle = True
        self.remote.clear()


# 创建小车对象
if __name__ == '__main__':
    car = Car('192.168.1.104')  # 此处传入ip地址
    car.start()
    time.sleep(10)  # 跑10秒
    car.clear()
    car.set_quit()  # 结束！
